﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Woodstock.Common;
using Phidgets;
using Phidgets.Events;
using System.ComponentModel;
using System.Collections.ObjectModel;

namespace Woodstock.HAL.Phidgets
{
    public class PhidgetSpatial : ISpatialStateProvider, INotifyPropertyChanged
    {
        private static Spatial spatial;

        private bool _attached;
        public bool Attached
        {
            get { return _attached; }
            set { _attached = value; NotifyPropertyChanged("Attached"); }
        }


        Woodstock.Common.Filters.KalmanFilter3 filterRoll;
        Woodstock.Common.Filters.KalmanFilter3 filterPitch;
        Woodstock.Common.Filters.KalmanFilter3 filterYaw;


        const int ROLL = 0;
        const int PITCH = 1;
        const int YAW = 2;
        const double g = 9.80665;

        public PhidgetSpatial()
        {
            if (spatial == null)
            {
                spatial = new Spatial();
                spatial.Attach += new AttachEventHandler(spatial_Attach);
                spatial.Detach += new DetachEventHandler(spatial_Detach);
                spatial.SpatialData += new SpatialDataEventHandler(spatial_SpatialData);

                spatial.open();


                // Creates the filtering algorithm
                filterRoll = new Common.Filters.KalmanFilter3();
                filterPitch = new Common.Filters.KalmanFilter3();
                filterYaw = new Common.Filters.KalmanFilter3();                
            }
            else
            {
                throw new Exception("Only one instance of PhidgetSpatial is allowed");
            }
        }

        void spatial_Detach(object sender, DetachEventArgs e)
        {
            Attached = false;
        }

        void spatial_Attach(object sender, global::Phidgets.Events.AttachEventArgs e)
        {
            Attached = true;
            spatial.DataRate = 8;
        }


        double[] lastMsCount = { 0, 0, 0 };
        bool[] lastMsCountGood = { false, false, false };
        double[] gyroHeading = { 0, 0, 0 }; //degrees


        void spatial_SpatialData(object sender, SpatialDataEventArgs e)
        {
            SpatialState result = new SpatialState();

            result.XAcceleration = e.spatialData[0].Acceleration[1];
            result.YAcceleration = e.spatialData[0].Acceleration[0];
            result.ZAcceleration = e.spatialData[0].Acceleration[2];

            result.XAngularRate = e.spatialData[0].AngularRate[0];
            result.YAngularRate = e.spatialData[0].AngularRate[1];
            result.ZAngularRate = e.spatialData[0].AngularRate[2];


            result.XAngle = calculateGyroHeading(e.spatialData, 0);
            result.YAngle = calculateGyroHeading(e.spatialData, 1);
            result.ZAngle = calculateGyroHeading(e.spatialData, 2);


            result.DateTime = DateTime.Now;



            //
            // Calculate filtered data
            //

            // Pitch OK
            {
                double gyro = e.spatialData[0].AngularRate[0];
                double accel = Math.Atan2(e.spatialData[0].Acceleration[1] / g, e.spatialData[0].Acceleration[2] / g);
                accel = (accel * 180) / Math.PI;

                result.Pitch = filterPitch.KalmanUpdate(gyro, accel);
            }


            // Roll
            {
                double gyro = e.spatialData[0].AngularRate[1];
                double accel = Math.Atan2(e.spatialData[0].Acceleration[0] / g, e.spatialData[0].Acceleration[2] / g);
                accel = (accel * 180) / Math.PI;

                result.Roll = filterRoll.KalmanUpdate(gyro, accel);
            }

            // Yaw
            /*
            {
                double gyro = e.spatialData[0].AngularRate[2];
                double accel = Math.Atan2(e.spatialData[0].Acceleration[0] / g, e.spatialData[0].Acceleration[2] / g);
                accel = (accel * 180) / Math.PI;

                result.Yaw = filterYaw.KalmanUpdate(gyro, accel);
            }*/            

            OnSpatialStateChanged(result);
        }

        //This integrates gyro angular rate into heading over time
        double calculateGyroHeading(SpatialEventData[] data, int index)
        {
            double gyro = 0;
            for (int i = 0; i < data.Length; i++)
            {
                gyro = data[i].AngularRate[index];

                if (lastMsCountGood[index])
                {
                    //calculate heading
                    double timechange = data[i].Timestamp.TotalMilliseconds - lastMsCount[index]; // in ms
                    double timeChangeSeconds = (double)timechange / 1000.0;

                    gyroHeading[index] += timeChangeSeconds * gyro;
                }

                lastMsCount[index] = data[i].Timestamp.TotalMilliseconds;
                lastMsCountGood[index] = true;
            }

            return gyroHeading[index];
        }


        public event PropertyChangedEventHandler PropertyChanged;

        protected void NotifyPropertyChanged(String info)
        {
            if (PropertyChanged != null)
            {
                PropertyChanged(this, new PropertyChangedEventArgs(info));
            }
        }

        #region ISpatialStateProvider Members

        public event EventHandler<SpatialStateEventArgs> SpatialStateChanged;

        public void OnSpatialStateChanged(SpatialState state)
        {
            if (SpatialStateChanged != null)
            {
                SpatialStateChanged(this, new SpatialStateEventArgs(state));
            }
        }

        #endregion
    }
}
